#pragma ident "$Id: SolverPPPUC.hpp 2648 2011-06-09 08:01:41Z shjzhang $"

/**
 * @file SolverPPPUC.hpp
 * General Solver.
 */

#ifndef GPSTK_SolverPPPUC_HPP
#define GPSTK_SolverPPPUC_HPP

//============================================================================
//
//  Revision
//
//  2019/08/13  
//  make a generic PPP solver for multiple-gnss data.  
//
//  2020/01/16 
//  modify the variable-class to replace the old ambiguity class  
//
//  shjzhang    
//
//============================================================================


#include "SolverBase.hpp"
#include "TypeID.hpp"
#include "ProcessingClass.hpp"
#include "EquationSystem.hpp"
#include "StochasticModel.hpp"
#include "SimpleKalmanFilter.hpp"


namespace gpstk
{

      /** @addtogroup GPSsolutions */
      /// @ingroup math

      // type definition for usage in other program
   typedef std::map<CommonTime, Vector<double>> epochVectorMap;
   typedef std::map<CommonTime, Matrix<double>> epochMatrixMap;

   class SolverPPPUC : public SolverBase, public ProcessingClass
   {
   public:

      SolverPPPUC()
          : firstTime(true), 
            useNEU(true), 
            forwardProcessing(true),  
            sigH(1.0E+8), sigLat(1.0E+8), sigLon(1.0E+8),
            sigX(1.0E+8), sigY(1.0E+8), sigZ(1.0E+8),
            pSolStreamFwd(NULL), pSolStreamSmoothed(NULL)

      {
          gpsTypeSet.clear();
          galTypeSet.clear();
          bdsTypeSet.clear();
          usingGPS = true;
          usingGAL = false;
          usingBDS = false;
          usingGLO = false;
          Init();
      }

         /** Returns a reference to a gnnsRinex object after solving
          *  the previously defined equation system.
          *
          * @param gData    Data object holding the data.
          */
      virtual void Process( commonHeader& comHeader, gnssRinex& gData )
         noexcept(false);

      
      virtual gnssRinex& Process(gnssRinex& gRin)
      {
          preCompute(gRin);
          Compute(gRin);
          postCompute(gRin);
          return gRin;
      };

         /** Code to be executed before 'Compute()' method.
          *
          * @param gData    Data object holding the data.
          */
      virtual gnssRinex& preCompute( gnssRinex& gRin )
         noexcept(false);


         /** Code to be executed after 'Compute()' method.
          *
          * @param gData    Data object holding the data.
          */
      virtual gnssRinex& postCompute( gnssRinex& gRin )
         noexcept(false);


         /** Returns a reference to a gnssRinex object after computing .
          *
          * @param gData    Data object holding the data.
          */
      virtual gnssRinex& Compute( gnssRinex& gRin )
         noexcept(false);


         /** Time Update of the kalman filter.
          *
          * @param gData    Data object holding the data.
          */
      virtual int TimeUpdate( const Matrix<double>& phiMatrix,
                              const Matrix<double>& processNoiseCovariance  )
         noexcept(false);


         /** Measurement Update of the kalman filter.
          *
          * @param gData    Data object holding the data.
          */
      virtual int MeasUpdate( const Vector<double>& prefitResiduals,
                              const Matrix<double>& designMatrix,
                              const Matrix<double>& weightMatrix  )
         noexcept(false);


      virtual void Constraint();


         /// This method resets the filter, setting all variance values in
         /// covariance matrix to a very high level.
      virtual SolverPPPUC& reset(void)
      { 
          firstTime = true;
          return (*this); 
      };

         /// neu.
      virtual SolverPPPUC& setUseNEU(bool neu)
      { useNEU = neu; return (*this); };

         /// This method set whether all the station is kinematic or not
      virtual SolverPPPUC& setKinematic(bool kin)
      { kinematic = kin; return (*this); };

         /// This method set whether all the station is kinematic or not
      virtual SolverPPPUC& setUConstraint(double sig)
      { sigH = sig; return (*this); };

      virtual SolverPPPUC& setNConstraint(double sig)
      { sigLat = sig; return (*this); };

      virtual SolverPPPUC& setEConstraint(double sig)
      { sigLon = sig; return (*this); };

      virtual SolverPPPUC& addType(const TypeID& type, const SatID::SatelliteSystem& sys)
      {
          if(sys==SatelliteSystem::GPS)
          {
              usingGPS = true;
              gpsTypeSet.insert(type);
          }
          else if(sys==SatelliteSystem::Galileo)
          {
              usingGAL = true;
              galTypeSet.insert(type);
          }
          else if(sys==SatelliteSystem::BDS)
          {
              usingBDS = true;
              bdsTypeSet.insert(type);
          }
          return (*this);
      };

         /// Returns an index identifying this object.
      virtual void Init(void) ;

         /// Returns an index identifying this object.
         /// PostProcess consists of backward-filtering and kalman-smoothing.
      virtual void PostProcess(commonHeader& comHeader) 
          noexcept(false);

         /// Returns an index identifying this object.
      virtual void setUpEquations(void) ;

      /// smooth solution using kalman-smoothed formula
      virtual void smoothSolution( Vector<double>& oldStateVec, 
                                   Matrix<double>& oldCovMatrix,
                                   Vector<double>& smoothedStateVec, 
                                   Matrix<double>& smoothedCovMatrix);


      SatIDSet extractSatSet(TypeID type,
                             SatelliteSystem::Systems system);

      VariableSet extractAmbSet(TypeID type,
                                SatelliteSystem::Systems system);

      virtual bool extractAmbData(Vector<double>& solution, 
                                  Matrix<double>& covMatrix,
                                  TypeID type,
                                  SatelliteSystem::Systems sys,
                                  VariableDataMap& ambMap,
                                  CovarianceDataMap& ambCovMap);

      virtual Variable getVariable( const TypeID& type, 
                                    const VariableSet& varSet) const
         noexcept(false);

         /** Returns the solution associated to a given TypeID.
          *
          * @param type    TypeID of the solution we are looking for.
          *
          * \warning In the case the solution contains more than one variable
          * of this type, only the first one will be returned.
          */
      virtual double getSolution( const TypeID& type,
                                  const Vector<double>& stateVec ) const
         noexcept(false);

  
      void setFwdOutStream(std::ofstream* pStream)
      {
          pSolStreamFwd = pStream;
      };

      void setSmoothedOutStream(std::ofstream* pStream)
      {
          pSolStreamSmoothed = pStream;
      };

      void setSmoothedAmbStream(std::ofstream* pStream)
      {
          pAmbStreamSmoothed = pStream;
      };


      void printSols( std::ofstream* outStr,
                      const CommonTime& epoch, 
                      const Vector<double>& stateVec,
                      const Matrix<double>& covMatrix,
                      int numSat);

      // print float solutions
      void printAmbs( std::ofstream* outStr, 
                      const CommonTime& epoch,
                      Vector<double>& solution,
                      Matrix<double>& covMatrix );

         /// Returns an index identifying this object.
      virtual int getIndex(void) const;

         /// Returns a string identifying this object.
      virtual std::string getClassName(void) const;

         /// Destructor.
      virtual ~SolverPPPUC() {};

   private:

      /// Boolean indicating if this filter was run at least once
      bool firstTime;

      /// is kinematic? 
      bool kinematic;

      /// bwd 
      bool forwardProcessing;

      /// use NEU or XYZ
      bool useNEU;
      bool neuConstraint;

      double sigH, sigLat, sigLon;
      double sigX, sigY, sigZ;

      bool usingGPS, usingGAL, usingBDS, usingGLO;

      TypeIDSet gpsTypeSet, galTypeSet, bdsTypeSet;

      VariableSet currentUnkSet;
      VariableSet oldUnkSet;

         /// Boolean indicating if this filter was run at least once
      CommonTime firstEpoch;

      std::ofstream* pSolStreamFwd;
      std::ofstream* pSolStreamSmoothed;
      std::ofstream* pAmbStreamSmoothed;

         /// State Transition Matrix (PhiMatrix)
      Matrix<double> phiMatrix;

         /// Noise covariance matrix (QMatrix)
      Matrix<double> qMatrix;

         /// Geometry matrix
      Matrix<double> hMatrix;

         /// Weights matrix
      Matrix<double> rMatrix;

         /// Measurements vector (Prefit-residuals)
      Vector<double> measVector;

         /// Map holding state information
      VariableDataMap stateMap;

         /// Map holding covariance information
      CovarianceDataMap covMap;

         // Predicted state
      Vector<double> xhatminus;

         // Predicted covariance.
      Matrix<double> Pminus;

         // A bwderiori state
      Vector<double> xhat;

         // A bwderiori covariance.
      Matrix<double> P;

      epochGnssRinexMap forwardRinexData;

      epochVectorMap forwardStateData;
      epochMatrixMap forwardCovData;
      epochPosMap nominalPosData;

      bool print;

      // default model for variable define
      WhiteNoiseModel defaultModel;

      // Declare coordinate model

      // kinematic
      // xyz
      WhiteNoiseModel whiteNoiseDXModel;
      WhiteNoiseModel whiteNoiseDYModel;
      WhiteNoiseModel whiteNoiseDZModel;
      // neu
      WhiteNoiseModel whiteNoiseDNModel;
      WhiteNoiseModel whiteNoiseDEModel;
      WhiteNoiseModel whiteNoiseDUModel;

      // Declare de/dn/du model
      // static positioning
      // xyz
      StochasticModel constantDXModel;
      StochasticModel constantDYModel;
      StochasticModel constantDZModel;
      // neu
      StochasticModel constantDNModel;
      StochasticModel constantDEModel;
      StochasticModel constantDUModel;

      // trop
      TropoRandomWalkModel tropoModel;

      IonoRandomWalkModel ionoModel;

      // cdt
      WhiteNoiseModel cdtModel;
      WhiteNoiseModel cdtGALModel;
      WhiteNoiseModel cdtBDSModel;

      WhiteNoiseModel cdtL1GPSModel;
      WhiteNoiseModel cdtL2GPSModel;
      WhiteNoiseModel cdtL5GPSModel;

      WhiteNoiseModel cdtL1GALModel;
      WhiteNoiseModel cdtL5GALModel;
      WhiteNoiseModel cdtL7GALModel;
      WhiteNoiseModel cdtL8GALModel;
      WhiteNoiseModel cdtL6GALModel;

      WhiteNoiseModel cdtL1BDSModel;
      WhiteNoiseModel cdtL2BDSModel;
      WhiteNoiseModel cdtL5BDSModel;
      WhiteNoiseModel cdtL7BDSModel;
      WhiteNoiseModel cdtL8BDSModel;
      WhiteNoiseModel cdtL6BDSModel;

      // LC ambiguities

      // GPS LC ambiguity
      PhaseAmbiguityModel GPSBL1Model;
      PhaseAmbiguityModel GPSBL2Model;
      PhaseAmbiguityModel GPSBL5Model;
                             
      // GAL LC ambiguity    
      PhaseAmbiguityModel GALBL1Model;
      PhaseAmbiguityModel GALBL5Model;
      PhaseAmbiguityModel GALBL7Model;
      PhaseAmbiguityModel GALBL8Model;
      PhaseAmbiguityModel GALBL6Model;
                             
      // BDS LC ambiguity    
      PhaseAmbiguityModel BDSBL1Model;
      PhaseAmbiguityModel BDSBL2Model;
      PhaseAmbiguityModel BDSBL5Model;
      PhaseAmbiguityModel BDSBL7Model;
      PhaseAmbiguityModel BDSBL8Model;
      PhaseAmbiguityModel BDSBL6Model;

         /// Equation system
      EquationSystem equSystem;

         /// Initial index assigned to this class.
      static int classIndex;

         /// Index belonging to this object.
      int index;

         /// Sets the index and increment classIndex.
      void setIndex(void)
      { index = classIndex++; };


   }; // End of class 'SolverPPPUC'


      //@}

}  // End of namespace gpstk
#endif   // GPSTK_SolverPPPUC_HPP
